#include "ISimulation.h"
#include "Receiver.h"


ISimulation::ISimulation(char* worldIrrFilePath){
	irrFilePath = worldIrrFilePath;

	receiver = new MyEventReceiver(device, camera, third_person_camera);
	
	device = createDevice(video::EDT_DIRECT3D9, core::dimension2d<u32>(800, 600), 16, false, false, false);

	if(device == 0)
		return;

	device->setWindowCaption(L"A* Pathfinding Simulation");
	driver = device->getVideoDriver();
}

ISimulation::~ISimulation()
{

}

void ISimulation::runSimulation(){
	loadWorld();
	m_start = vector3df(700, 0, -770);
	initializeCamera();
	initializePathfinder();
	receiver->SetDevice(device);
	receiver->SetCameras(camera, third_person_camera);
	device->setEventReceiver(receiver);
	
	int lastFPS = -1;
	const f32 MOVEMENT_SPEED = 5.f;


	u32 then = device->getTimer()->getTime();
	vector3df currentGoal;
	m_Agent->SetPosition(m_start);
	m_Agent->MoveTo(m_currentPath.front());
	m_currentPath.pop_front();
	device->maximizeWindow();
	while(device->run())
	{
		if (device->isWindowActive())
		{
			const u32 now = device->getTimer()->getTime();
			const f32 frameDeltaTime = (f32)(now - then) / 1000.f; // Time in seconds
			then = now;

			if(receiver->Get_Clear() & path_ids.size() > 0)
			{
				Clear_Path();
				receiver->Set_Clear();
			}

			driver->beginScene(true, true, video::SColor(0,200,200,200));

			m_Agent->Update(frameDeltaTime);
			if(m_Agent->IsIdle() == true)
			{
				if(m_currentPath.size() != 0)
				{

					vector3df newGoal = m_currentPath.front();
					m_Agent->MoveTo(newGoal);
					m_currentPath.pop_front();
				}
			}
			UpdateCamera();
			device->getSceneManager()->drawAll();
			device->getGUIEnvironment()->drawAll();
			
			if(receiver->Get_Reload())
			{
				Restart();
			}

			core::stringw caption = "Current Camera Position: ";
			caption += "<x,y,z> : <";
			if(device->getSceneManager()->getActiveCamera() == camera)
			{
				caption += camera->getPosition().X;
				caption += ",";
				caption += camera->getPosition().Y;
				caption += ",";
				caption += camera->getPosition().Z;
				caption += ">";
			}
			else
			{
				caption += third_person_camera->getPosition().X;
				caption += ",";
				caption += third_person_camera->getPosition().Y;
				caption += ",";
				caption += third_person_camera->getPosition().Z;
				caption += ">";
			}
			device->setWindowCaption(caption.c_str());

			int fps = driver->getFPS();
			if (lastFPS != fps)
			{
				lastFPS = fps;
			}
			driver->endScene();
		}
	}

    device->drop();

	return;

}

///Loads Irrlich file file
void ISimulation::loadWorld(){
	scene::ISceneManager* smgr = device->getSceneManager();
	smgr->loadScene(irrFilePath);
	//CreateObjects();
	m_Agent = new Agent(smgr, "../../../Agent/DaetoxFile/agent.x", core::vector3df(2,0,2), core::vector3df(0,0,0));
}

///Creates and initializes the world camera
void ISimulation::initializeCamera(){
	camera = device->getSceneManager()->addCameraSceneNode(0,vector3df(0, 250, 0),m_Agent->GetPosition());

	third_person_camera = device->getSceneManager()->addCameraSceneNodeFPS(0, 7, 0.5);
	third_person_camera->setPosition(vector3df(70,350,-70));
	third_person_camera->setTarget(m_start);

	device->getSceneManager()->setActiveCamera(third_person_camera);

}

void ISimulation::initializePathfinder(){
	
	
	m_goal = vector3df(0,0,800);
	m_planner = new IPathPlanner();
	
	scene::ISceneManager* smgr = device->getSceneManager();
	core::array<scene::ISceneNode*> nodes;
	smgr->getSceneNodesFromType(scene::ESNT_MESH, nodes); 
	
	core::stringw node_name;

	for (u32 i=0; i < nodes.size(); ++i) {
		scene::ISceneNode* node = nodes[i];

		node_name = node->getName();
		if(node_name == "ROOM")
		{
			m_planner->LoadLevel(node);
			break;
		}
	}

	for (u32 i=0; i < nodes.size(); ++i) {
		scene::ISceneNode* node = nodes[i];

		node_name = node->getName();
		if(node_name != "ROOM")
		{
			node->setScale(node->getScale());
			node->setDebugDataVisible(scene::EDS_BBOX);
			m_planner->AddObstacle(node);
		}
	}

	
	core::vector2df start(m_start.X,m_start.Z);
	core::vector2df target(m_goal.X, m_goal.Z);
	m_planner->PlotPathToTarget(start, target, m_currentPath);
	
	/*
	printf("Starting at <%3.2f, %3.2f, %3.2f> with target of <%3.2f, %3.2f, %3.2f>\n", start.X, 0.f, start.Y, target.X, 0.f, target.Y);
	std::list<vector3df>::iterator iter;
	for(iter = m_currentPath.begin(); iter != m_currentPath.end(); ++iter)
	{
		printf("Move to <%3.2f, %3.2f, %3.2f>\n", iter->X, iter->Y, iter->Z);
	}*/

}

void ISimulation::UpdateCamera()
{
	matrix4 camera_Matrix;
	vector3df new_camera_position;
	vector3df agent_position = m_Agent->GetPosition();
	vector3df camera_offset = vector3df(0, 275, -225);
	vector3df agent_rotation = m_Agent->GetRotation();

	camera_Matrix.setRotationDegrees(-agent_rotation);

	new_camera_position.X = ((camera_Matrix[0] * camera_offset.X) + (camera_Matrix[1] * camera_offset.Y) +
		(camera_Matrix[2] * camera_offset.Z)) + agent_position.X;

	new_camera_position.Y = ((camera_Matrix[4] * camera_offset.X) + (camera_Matrix[5] * camera_offset.Y) +
		(camera_Matrix[6] * camera_offset.Z)) + agent_position.Y;

	new_camera_position.Z = ((camera_Matrix[8] * camera_offset.X) + (camera_Matrix[9] * camera_offset.Y) +
		(camera_Matrix[10] * camera_offset.Z)) + agent_position.Z;

	if(new_camera_position.Z < -850) { new_camera_position.Z = -849; }

	if(new_camera_position.Z > 850) { new_camera_position.Z = 849; }

	if(new_camera_position.X < -850) { new_camera_position.X = -849; }

	if(new_camera_position.X > 850) { new_camera_position.X = 849; }

	camera->setPosition(new_camera_position);

	camera->setTarget(vector3df(agent_position.X, agent_position.Y + 50, agent_position.Z) );


	//third_person_camera->setTarget(agent_position);

	
}

void ISimulation::CreateObjects()
{
	scene::ISceneManager* smgr = device->getSceneManager();

	scene::IMesh* plant1_mesh = smgr->getMesh("../../../SketchUp/obj_files/plant1.x");
	IMeshSceneNode* plant1 = smgr->addMeshSceneNode(plant1_mesh);
	plant1->setPosition(vector3df(642,0,-550));
	plant1->setScale(vector3df(60,60,60));
	plant1->setID(-666);
	plant1->setVisible(true);
}

void ISimulation::Restart()
{
	m_planner->~IPathPlanner();
	m_currentPath.clear();

	

	int cycle = receiver->Get_Cycle();

	switch(cycle)
	{
	case 1:
		m_start = vector3df(-700, 0, -770);
		break;
	case 2:
		m_start = vector3df(0, 0, 0);
		break;
	case 3:
		m_start = vector3df(-700, 0, 600);
		break;
	default:
		m_start = vector3df(700, 0, -770);
		break;
	}

	initializePathfinder();
	
	if(receiver->Get_Draw_Path())
	{
		Draw_Path();
	}

	m_Agent->SetPosition(m_start);
	device->getSceneManager()->getSceneNodeFromName("AGENT")->setRotation(vector3df(0,0,0));
	m_Agent->MoveTo(m_currentPath.front());
	m_currentPath.pop_front();
	

	receiver->Set_Reload(false);
}

void ISimulation::Clear_Path()
{
	int amount = receiver->Get_Size();

	for (u32 i=0; i < amount; ++i) 
	{
		device->getSceneManager()->getSceneNodeFromId(path_ids[i])->remove();		
	}

	path_ids.clear();

}

void ISimulation::Draw_Path()
{
	if(path_ids.size() > 0)
	{
		Clear_Path();
		receiver->Set_Clear();
	}

	for(int i = 0; i < m_currentPath.size(); i++)
	{
		ISceneNode* node = device->getSceneManager()->addSphereSceneNode(12.5);
		vector3df temp_pos = m_currentPath.front();
		node->setPosition(temp_pos);
		
		int temp_id = 10000 + i;
		node->setID(temp_id);
		path_ids.push_front(node->getID());
	
		node->setMaterialTexture(0,  driver->getTexture("../../../textures/path.jpg"));

		node->setMaterialFlag(video::EMF_NORMALIZE_NORMALS, true);
		node->setMaterialFlag(video::EMF_LIGHTING, false);
		node->setMaterialType(video::EMT_SOLID);

		m_currentPath.pop_front();
		m_currentPath.push_back(temp_pos);
	}

	receiver->Set_Size(path_ids.size());
}

void ISimulation::printDebugging()
{
	
}

